/**
 * @file sensor_collector.c
 * @brief 传感器数据采集模块实现
 * 
 * 该模块实现定时采集各线程传感器数据，并提供快速访问接口。
 * 使用读写锁保证线程安全，支持细粒度锁控制。
 * 
 * @author 伞控无人机项目组
 * @date 2024
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <time.h>
#include <errno.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include "sensor_collector.h"
#include "log_utils.h"
#include "log_modules.h"

// 配置常量
#define SENSOR_COLLECTION_INTERVAL 3        // 数据采集间隔（秒）
#define SENSOR_DATA_TIMEOUT 10              // 传感器数据超时时间（秒）
#define MAX_RETRY_COUNT 3                   // 最大重试次数
#define SHM_KEY_BASE 0x1234                 // 共享内存键值基址

// 共享内存键值定义
#define THREAD4_SHM_KEY (SHM_KEY_BASE + 4)  // 线程4共享内存键值
#define THREAD5_SHM_KEY (SHM_KEY_BASE + 5)  // 线程5共享内存键值
#define THREAD8_SHM_KEY (SHM_KEY_BASE + 8)  // 线程8共享内存键值
#define THREAD9_SHM_KEY (SHM_KEY_BASE + 9)  // 线程9共享内存键值

// 全局变量
static volatile int g_sensor_collector_running = 1;  // 线程运行标志
static global_sensor_data_t g_sensor_data;           // 全局传感器数据表

// 共享内存指针
static void *g_thread4_shm = NULL;
static void *g_thread5_shm = NULL;
static void *g_thread8_shm = NULL;
static void *g_thread9_shm = NULL;

// 内部函数声明
static int init_shared_memory(void);
static void cleanup_shared_memory(void);
static void collect_prox1_data(void);
static void collect_prox2_data(void);
static void collect_nav_data(void);
static void collect_rs485_data(void);
static void update_global_timestamp(void);
static int get_prox1_distance(void);
static int get_prox1_status(void);
static int get_prox2_distance(void);
static int get_prox2_status(void);
static double get_nav_latitude(void);
static double get_nav_longitude(void);
static float get_nav_altitude(void);
static float get_nav_heading(void);
static float get_rs485_value1(void);
static float get_rs485_value2(void);
static int get_rs485_status(void);
static void log_sensor_data_update(sensor_type_t type);

// 传感器类型名称
static const char* sensor_type_names[] = {
    "PROX1",    // 接近传感器1
    "PROX2",    // 接近传感器2
    "NAV",      // 导航系统
    "RS485"     // RS485传感器
};

/**
 * @brief 初始化传感器数据表
 */
int init_sensor_data_table(void) {
    // 初始化传感器状态数组
    for (int i = 0; i < SENSOR_COUNT; i++) {
        g_sensor_data.sensors[i].type = i;
        g_sensor_data.sensors[i].is_valid = 0;
        g_sensor_data.sensors[i].last_update = 0;
        g_sensor_data.sensors[i].error_count = 0;
        
        // 初始化读写锁
        if (pthread_rwlock_init(&g_sensor_data.sensor_locks[i], NULL) != 0) {
            LOG_ERROR(LOG_TO_FILE, LOG_MODULE_SENSOR, 
                     "初始化传感器 %s 读写锁失败", sensor_type_names[i]);
            return -1;
        }
    }
    
    // 初始化全局变量
    g_sensor_data.last_full_update = time(NULL);
    g_sensor_data.update_count = 0;
    g_sensor_data.total_error_count = 0;
    
    // 初始化共享内存
    if (init_shared_memory() < 0) {
        LOG_ERROR(LOG_TO_FILE, LOG_MODULE_SENSOR, "初始化共享内存失败");
        return -1;
    }
    
    LOG_INFO(LOG_TO_FILE, LOG_MODULE_SENSOR, "传感器数据表初始化成功");
    return 0;
}

/**
 * @brief 清理传感器数据表
 */
void cleanup_sensor_data_table(void) {
    // 销毁读写锁
    for (int i = 0; i < SENSOR_COUNT; i++) {
        pthread_rwlock_destroy(&g_sensor_data.sensor_locks[i]);
    }
    
    // 清理共享内存
    cleanup_shared_memory();
    
    LOG_INFO(LOG_TO_FILE, LOG_MODULE_SENSOR, "传感器数据表清理完成");
}

/**
 * @brief 初始化共享内存
 */
static int init_shared_memory(void) {
    // 尝试连接现有的共享内存段
    int shm_id4 = shmget(THREAD4_SHM_KEY, 1024, 0666);
    int shm_id5 = shmget(THREAD5_SHM_KEY, 1024, 0666);
    int shm_id8 = shmget(THREAD8_SHM_KEY, 1024, 0666);
    int shm_id9 = shmget(THREAD9_SHM_KEY, 1024, 0666);
    
    if (shm_id4 < 0 || shm_id5 < 0 || shm_id8 < 0 || shm_id9 < 0) {
        LOG_WARN(LOG_TO_FILE, LOG_MODULE_SENSOR, 
                "部分共享内存段不存在，将使用模拟数据");
    }
    
    // 映射共享内存（如果存在）
    if (shm_id4 >= 0) {
        g_thread4_shm = shmat(shm_id4, NULL, 0);
        if (g_thread4_shm == (void*)-1) {
            LOG_WARN(LOG_TO_FILE, LOG_MODULE_SENSOR, "线程4共享内存映射失败");
            g_thread4_shm = NULL;
        }
    }
    
    if (shm_id5 >= 0) {
        g_thread5_shm = shmat(shm_id5, NULL, 0);
        if (g_thread5_shm == (void*)-1) {
            LOG_WARN(LOG_TO_FILE, LOG_MODULE_SENSOR, "线程5共享内存映射失败");
            g_thread5_shm = NULL;
        }
    }
    
    if (shm_id8 >= 0) {
        g_thread8_shm = shmat(shm_id8, NULL, 0);
        if (g_thread8_shm == (void*)-1) {
            LOG_WARN(LOG_TO_FILE, LOG_MODULE_SENSOR, "线程8共享内存映射失败");
            g_thread8_shm = NULL;
        }
    }
    
    if (shm_id9 >= 0) {
        g_thread9_shm = shmat(shm_id9, NULL, 0);
        if (g_thread9_shm == (void*)-1) {
            LOG_WARN(LOG_TO_FILE, LOG_MODULE_SENSOR, "线程9共享内存映射失败");
            g_thread9_shm = NULL;
        }
    }
    
    return 0;
}

/**
 * @brief 清理共享内存
 */
static void cleanup_shared_memory(void) {
    if (g_thread4_shm) {
        shmdt(g_thread4_shm);
        g_thread4_shm = NULL;
    }
    
    if (g_thread5_shm) {
        shmdt(g_thread5_shm);
        g_thread5_shm = NULL;
    }
    
    if (g_thread8_shm) {
        shmdt(g_thread8_shm);
        g_thread8_shm = NULL;
    }
    
    if (g_thread9_shm) {
        shmdt(g_thread9_shm);
        g_thread9_shm = NULL;
    }
}

/**
 * @brief 传感器数据采集线程主函数
 */
void *sensor_data_collector_function(void *arg) {
    (void)arg;
    LOG_INFO(LOG_TO_FILE, LOG_MODULE_SENSOR, "传感器数据采集线程启动");
    
    // 初始化传感器数据表
    if (init_sensor_data_table() < 0) {
        LOG_ERROR(LOG_TO_FILE, LOG_MODULE_SENSOR, "初始化传感器数据表失败");
        return NULL;
    }
    
    // 主循环：每3秒采集一次数据
    while (g_sensor_collector_running) {
        time_t start_time = time(NULL);
        
        LOG_DEBUG(LOG_TO_CONSOLE_ONLY, LOG_MODULE_SENSOR, "开始采集传感器数据...");
        
        // 采集各线程传感器数据
        collect_prox1_data();      // 线程4数据
        collect_prox2_data();      // 线程5数据
        collect_nav_data();        // 线程8数据
        collect_rs485_data();      // 线程9数据
        
        // 更新全局时间戳
        update_global_timestamp();
        
        LOG_DEBUG(LOG_TO_CONSOLE_ONLY, LOG_MODULE_SENSOR, 
                 "传感器数据采集完成，更新计数: %d", g_sensor_data.update_count);
        
        // 等待到下一个3秒周期
        time_t elapsed = time(NULL) - start_time;
        if (elapsed < SENSOR_COLLECTION_INTERVAL) {
            sleep(SENSOR_COLLECTION_INTERVAL - elapsed);
        }
    }
    
    cleanup_sensor_data_table();
    LOG_INFO(LOG_TO_FILE, LOG_MODULE_SENSOR, "传感器数据采集线程退出");
    return NULL;
}

/**
 * @brief 停止传感器数据采集线程
 */
void sensor_collector_stop(void) {
    LOG_INFO(LOG_TO_FILE, LOG_MODULE_SENSOR, "传感器数据采集线程停止中");
    g_sensor_collector_running = 0;
}

/**
 * @brief 采集接近传感器1数据（线程4）
 */
static void collect_prox1_data(void) {
    // 获取写锁
    pthread_rwlock_wrlock(&g_sensor_data.sensor_locks[SENSOR_PROX1]);
    
    sensor_status_t *prox1 = &g_sensor_data.sensors[SENSOR_PROX1];
    
    // 更新数据
    prox1->data.prox_data.distance = get_prox1_distance();
    prox1->data.prox_data.status = get_prox1_status();
    prox1->data.prox_data.timestamp = time(NULL);
    prox1->is_valid = 1;
    prox1->last_update = time(NULL);
    
    // 释放写锁
    pthread_rwlock_unlock(&g_sensor_data.sensor_locks[SENSOR_PROX1]);
    
    log_sensor_data_update(SENSOR_PROX1);
}

/**
 * @brief 采集接近传感器2数据（线程5）
 */
static void collect_prox2_data(void) {
    // 获取写锁
    pthread_rwlock_wrlock(&g_sensor_data.sensor_locks[SENSOR_PROX2]);
    
    sensor_status_t *prox2 = &g_sensor_data.sensors[SENSOR_PROX2];
    
    // 更新数据
    prox2->data.prox_data.distance = get_prox2_distance();
    prox2->data.prox_data.status = get_prox2_status();
    prox2->data.prox_data.timestamp = time(NULL);
    prox2->is_valid = 1;
    prox2->last_update = time(NULL);
    
    // 释放写锁
    pthread_rwlock_unlock(&g_sensor_data.sensor_locks[SENSOR_PROX2]);
    
    log_sensor_data_update(SENSOR_PROX2);
}

/**
 * @brief 采集导航数据（线程8）
 */
static void collect_nav_data(void) {
    // 获取写锁
    pthread_rwlock_wrlock(&g_sensor_data.sensor_locks[SENSOR_NAV]);
    
    sensor_status_t *nav = &g_sensor_data.sensors[SENSOR_NAV];
    
    // 更新数据
    nav->data.nav_data.latitude = get_nav_latitude();
    nav->data.nav_data.longitude = get_nav_longitude();
    nav->data.nav_data.altitude = get_nav_altitude();
    nav->data.nav_data.heading = get_nav_heading();
    nav->data.nav_data.pitch = 0.0f;  // 暂时设为0
    nav->data.nav_data.roll = 0.0f;   // 暂时设为0
    nav->data.nav_data.timestamp = time(NULL);
    nav->is_valid = 1;
    nav->last_update = time(NULL);
    
    // 释放写锁
    pthread_rwlock_unlock(&g_sensor_data.sensor_locks[SENSOR_NAV]);
    
    log_sensor_data_update(SENSOR_NAV);
}

/**
 * @brief 采集RS485数据（线程9）
 */
static void collect_rs485_data(void) {
    // 获取写锁
    pthread_rwlock_wrlock(&g_sensor_data.sensor_locks[SENSOR_RS485]);
    
    sensor_status_t *rs485 = &g_sensor_data.sensors[SENSOR_RS485];
    
    // 更新数据
    rs485->data.rs485_data.value1 = get_rs485_value1();
    rs485->data.rs485_data.value2 = get_rs485_value2();
    rs485->data.rs485_data.status = get_rs485_status();
    rs485->data.rs485_data.timestamp = time(NULL);
    rs485->is_valid = 1;
    rs485->last_update = time(NULL);
    
    // 释放写锁
    pthread_rwlock_unlock(&g_sensor_data.sensor_locks[SENSOR_RS485]);
    
    log_sensor_data_update(SENSOR_RS485);
}

/**
 * @brief 更新全局时间戳
 */
static void update_global_timestamp(void) {
    g_sensor_data.last_full_update = time(NULL);
    g_sensor_data.update_count++;
}

/**
 * @brief 记录传感器数据更新日志
 */
static void log_sensor_data_update(sensor_type_t type) {
    LOG_DEBUG(LOG_TO_CONSOLE_ONLY, LOG_MODULE_SENSOR, 
             "传感器 %s 数据更新完成", sensor_type_names[type]);
}

// 模拟数据获取函数（实际应从共享内存或消息队列获取）
static int get_prox1_distance(void) {
    // 模拟距离数据 (100-1000mm)
    return 100 + (rand() % 900);
}

static int get_prox1_status(void) {
    // 模拟状态数据 (0=正常, 1=异常)
    return (rand() % 100) < 95 ? 0 : 1;
}

static int get_prox2_distance(void) {
    // 模拟距离数据 (100-1000mm)
    return 100 + (rand() % 900);
}

static int get_prox2_status(void) {
    // 模拟状态数据 (0=正常, 1=异常)
    return (rand() % 100) < 95 ? 0 : 1;
}

static double get_nav_latitude(void) {
    // 模拟纬度数据 (30.0-31.0度)
    return 30.0 + (rand() % 100) / 100.0;
}

static double get_nav_longitude(void) {
    // 模拟经度数据 (120.0-121.0度)
    return 120.0 + (rand() % 100) / 100.0;
}

static float get_nav_altitude(void) {
    // 模拟高度数据 (0-1000m)
    return (rand() % 1000);
}

static float get_nav_heading(void) {
    // 模拟航向数据 (0-360度)
    return (rand() % 360);
}

static float get_rs485_value1(void) {
    // 模拟RS485数值1
    return (rand() % 1000) / 10.0f;
}

static float get_rs485_value2(void) {
    // 模拟RS485数值2
    return (rand() % 1000) / 10.0f;
}

static int get_rs485_status(void) {
    // 模拟RS485状态 (0=正常, 1=异常)
    return (rand() % 100) < 90 ? 0 : 1;
}

/**
 * @brief 获取所有传感器数据（线程6使用）
 */
int get_all_sensor_data(global_sensor_data_t *data_copy) {
    if (!data_copy) return -1;
    
    // 逐个获取传感器数据（细粒度锁）
    for (int i = 0; i < SENSOR_COUNT; i++) {
        pthread_rwlock_rdlock(&g_sensor_data.sensor_locks[i]);
        data_copy->sensors[i] = g_sensor_data.sensors[i];
        pthread_rwlock_unlock(&g_sensor_data.sensor_locks[i]);
    }
    
    // 复制全局信息
    data_copy->last_full_update = g_sensor_data.last_full_update;
    data_copy->update_count = g_sensor_data.update_count;
    data_copy->total_error_count = g_sensor_data.total_error_count;
    
    return 0;
}

/**
 * @brief 获取特定传感器数据
 */
int get_sensor_data(sensor_type_t type, sensor_status_t *data) {
    if (type >= SENSOR_COUNT || !data) return -1;
    
    pthread_rwlock_rdlock(&g_sensor_data.sensor_locks[type]);
    *data = g_sensor_data.sensors[type];
    pthread_rwlock_unlock(&g_sensor_data.sensor_locks[type]);
    
    return 0;
}

/**
 * @brief 获取传感器数据快照（用于RS232发送）
 */
int get_sensor_collector_snapshot(sensor_data_snapshot_t *snapshot) {
    if (!snapshot) return -1;
    
    // 初始化快照
    snapshot->timestamp = time(NULL);
    snapshot->data_count = 0;
    
    // 获取接近传感器1数据
    sensor_status_t prox1_data;
    if (get_sensor_data(SENSOR_PROX1, &prox1_data) == 0 && prox1_data.is_valid) {
        snapshot->prox1_distance = prox1_data.data.prox_data.distance;
        snapshot->prox1_status = prox1_data.data.prox_data.status;
        snapshot->data_count++;
    }
    
    // 获取接近传感器2数据
    sensor_status_t prox2_data;
    if (get_sensor_data(SENSOR_PROX2, &prox2_data) == 0 && prox2_data.is_valid) {
        snapshot->prox2_distance = prox2_data.data.prox_data.distance;
        snapshot->prox2_status = prox2_data.data.prox_data.status;
        snapshot->data_count++;
    }
    
    // 获取导航数据
    sensor_status_t nav_data;
    if (get_sensor_data(SENSOR_NAV, &nav_data) == 0 && nav_data.is_valid) {
        snapshot->nav_latitude = nav_data.data.nav_data.latitude;
        snapshot->nav_longitude = nav_data.data.nav_data.longitude;
        snapshot->nav_altitude = nav_data.data.nav_data.altitude;
        snapshot->nav_heading = nav_data.data.nav_data.heading;
        snapshot->data_count++;
    }
    
    // 获取RS485数据
    sensor_status_t rs485_data;
    if (get_sensor_data(SENSOR_RS485, &rs485_data) == 0 && rs485_data.is_valid) {
        snapshot->rs485_value1 = rs485_data.data.rs485_data.value1;
        snapshot->rs485_value2 = rs485_data.data.rs485_data.value2;
        snapshot->rs485_status = rs485_data.data.rs485_data.status;
        snapshot->data_count++;
    }
    
    return 0;
}

/**
 * @brief 获取传感器类型名称
 */
const char* get_sensor_type_name(sensor_type_t type) {
    if (type >= SENSOR_COUNT) return "UNKNOWN";
    return sensor_type_names[type];
}

/**
 * @brief 获取传感器数据有效性
 */
int is_sensor_collector_data_valid(sensor_type_t type) {
    if (type >= SENSOR_COUNT) return 0;
    
    sensor_status_t data;
    if (get_sensor_data(type, &data) == 0) {
        return data.is_valid;
    }
    return 0;
}

/**
 * @brief 获取最后更新时间
 */
time_t get_sensor_last_update(sensor_type_t type) {
    if (type >= SENSOR_COUNT) return 0;
    
    sensor_status_t data;
    if (get_sensor_data(type, &data) == 0) {
        return data.last_update;
    }
    return 0;
} 